/**
 * Big end
 */
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/sys/byteorder.h>

#include "hwap_protocol.h"
#include "serial.h"

#define LOG_MODULE_NAME HWAP
LOG_MODULE_REGISTER(LOG_MODULE_NAME, LOG_LEVEL_DBG);

#define BUFFER_SIZE 1024

typedef enum
{
    STATE_MAGIC,
    STATE_SEQ,
    STATE_OP,
    STATE_TYPE,
    STATE_LEN,
    STATE_PAYLOAD,
    STATE_CRC,
    STATE_DONE,
    STATE_ERROR
} hwap_state_t;

static uint8_t recv_buffer[BUFFER_SIZE];
static uint8_t send_buffer[BUFFER_SIZE];

struct hwap_pkt decoding_pkt;

extern struct serial_inst *serial_inst;

static unsigned short crc16(unsigned short *buf, int size);

static void on_check_version_rsp(void)
{
    memset(send_buffer, 0, sizeof(send_buffer));
    struct hwap_pkt *dec_pkt = (struct hwap_pkt *)send_buffer;
    dec_pkt->magic = BSWAP_32(HWAP_MAGIC);
    dec_pkt->seq = BSWAP_16(0x2000);
    dec_pkt->op = BSWAP_16(0x0001);
    dec_pkt->type = BSWAP_16(0x1002);
    dec_pkt->payload_len = BSWAP_16(0x3D);

    send_buffer[12] = PROTOCOL_VERSION;
    send_buffer[13] = 0x66; // {0x66, 0x84, 0x53, 0x46, 0x48}
    send_buffer[14] = 0x84;
    send_buffer[15] = 0x53;
    send_buffer[16] = 0x46;
    send_buffer[17] = 0x48;

    send_buffer[23] = 0x32;
    send_buffer[24] = 0x2E;
    send_buffer[25] = 0x34;
    send_buffer[26] = 0x47;

    uint16_t c_crc = crc16((unsigned short *)send_buffer, 73);
    send_buffer[73] = (c_crc >> 8) & 0xFF;
    send_buffer[74] = c_crc & 0xFF;

    LOG_HEXDUMP_DBG(send_buffer, 75, "tx enc");
    // serial_inst->tx(send_buffer, 75);
}

static void data_handler(const uint8_t *const data, uint16_t data_len)
{
    struct hwap_pkt *dec_pkt = (struct hwap_pkt *)data;
    LOG_HEXDUMP_DBG(data, data_len, "HWAP RX");
    LOG_DBG("dec_pkt->type: 0x%04X", BSWAP_16(dec_pkt->type));
    switch (BSWAP_16(dec_pkt->type))
    {
    case 0x1001: // 版本查询
        on_check_version_rsp();
        break;

    default:
        break;
    }
}

static unsigned short crc16(unsigned short *buf, int size)
{
    int sum = 0;
    unsigned short *p = buf;
    unsigned short last = 0;

    while (size > 1)
    {
        sum += *p++;
        size -= 2;
    }

    if (size == 1)
    {
        *(unsigned char *)(&last) = *(unsigned char *)p;
        sum += last;
    }

    sum = (sum >> 16) + (sum & 0xFFFF);
    sum += (sum >> 16);
    last = ~sum;

    return last;
}

void hwap_decode_byte(const uint8_t *const data, uint16_t data_len)
{
    static hwap_state_t state = STATE_MAGIC;
    uint16_t pkt_seq = 0;
    uint16_t pkt_op = 0;
    uint16_t pkt_type = 0;
    uint16_t pkt_payload_len = 0;
    static uint16_t buffer_pos = 0; // 当前接收数据的偏移量
    static uint8_t magic_match_index = 0;
    const uint8_t magic_bytes[] = MAGIC_BYTES;

    for (int i = 0; i < data_len; i++)
    {
        uint8_t byte = data[i];
        // LOG_DBG("len: %d, state: %d, pos: %d, byte: 0x%02X", data_len, state, buffer_pos, byte);
        switch (state)
        {
        case STATE_MAGIC:
        {
            if (byte == magic_bytes[magic_match_index])
            {
                magic_match_index++;
                if (magic_match_index == sizeof(magic_bytes))
                {
                    magic_match_index = 0;
                    memcpy(recv_buffer, magic_bytes, sizeof(magic_bytes));
                    buffer_pos = 4;
                    state = STATE_SEQ;
                }
            }
            else
            {
                magic_match_index = 0;
            }
        }
        break;

        case STATE_SEQ:
        {
            if (buffer_pos < sizeof(recv_buffer))
            {
                recv_buffer[buffer_pos++] = byte;
            }
            if (buffer_pos == 6)
            {
                memcpy(&pkt_seq, recv_buffer + 4, 2);
                pkt_seq = BSWAP_16(pkt_seq);
                state = STATE_OP;
                // LOG_DBG("pkt_seq: 0x%04X", pkt_seq);
            }
        }
        break;

        case STATE_OP:
        {
            if (buffer_pos < sizeof(recv_buffer))
            {
                recv_buffer[buffer_pos++] = byte;
            }

            if (buffer_pos == 8)
            {
                memcpy(&pkt_op, recv_buffer + 6, 2);
                pkt_op = BSWAP_16(pkt_op);
                state = STATE_TYPE;
                // LOG_DBG("pkt_op: 0x%04X", pkt_op);
            }
        }
        break;

        case STATE_TYPE:
        {
            if (buffer_pos < sizeof(recv_buffer))
            {
                recv_buffer[buffer_pos++] = byte;
            }

            if (buffer_pos == 10)
            {
                memcpy(&pkt_type, recv_buffer + 8, 2);
                pkt_type = BSWAP_16(pkt_type);
                state = STATE_LEN;
                // LOG_DBG("pkt_type: 0x%04X", pkt_type);
            }
        }
        break;

        case STATE_LEN:
        {
            if (buffer_pos < sizeof(recv_buffer))
            {
                recv_buffer[buffer_pos++] = byte;
            }

            if (buffer_pos == 12)
            {
                memcpy(&pkt_payload_len, recv_buffer + 10, 2);
                pkt_payload_len = BSWAP_16(pkt_payload_len);
                // LOG_DBG("pkt_payload_len: %d(0x%04X)", pkt_payload_len, pkt_payload_len);
                if (pkt_payload_len <= MAX_PACKET_SIZE)
                {
                    state = STATE_PAYLOAD;
                }
                else
                {
                    LOG_ERR("Packet length exceeds MAX_PACKET_SIZE: %d", pkt_payload_len);
                    buffer_pos = 0;
                    state = STATE_MAGIC;
                }
            }
        }
        break;

        case STATE_PAYLOAD:
        {
            if (buffer_pos < sizeof(recv_buffer))
            {
                recv_buffer[buffer_pos++] = byte;
            }

            if (buffer_pos == (12 + pkt_payload_len + 2))
            {
                uint16_t expected_crc = 0;
                memcpy(&expected_crc, recv_buffer + 12 + pkt_payload_len, 2);
                expected_crc = BSWAP_16(expected_crc);
                uint16_t calculated_crc = crc16((unsigned short *)recv_buffer, 12 + pkt_payload_len);
                if (calculated_crc == expected_crc)
                {
                    // LOG_DBG("CRC check passed");
                    data_handler(recv_buffer, 12 + pkt_payload_len);
                }
                else
                {
                    LOG_ERR("CRC check failed, expected_crc: 0x%04X, calculated_crc: 0x%04X", expected_crc, calculated_crc);
                }

                // reset
                buffer_pos = 0;
                state = STATE_MAGIC;
            }
        }
        break;

        default:
            break;
        }
    }
}
